﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Linq;
using System.Windows.Forms;
using System.Net.Sockets;
using System.Drawing.Imaging;
using System.Threading;
using System.Diagnostics;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Util;

namespace NAO_Image_Processing
{
    public partial class GUI : Form
    {
        /* Statischer Block */
        private static int KCOLORSPACE = 13;

        /* Klassendeklaration */
        private NAOControl _naoControl;
        private NAOCamera _camera;
        private ImageProcessor _imageProcessor;

        /* Variablendeklaration */
        private bool _naoCamInitialised;
        private bool updatingPicture;
        private NaoCamImageFormat _naoCamCurrentImageFormat;
        private Bitmap bitmap;
        private Image<Bgr, Byte> imgOrig;
        private Image<Bgr, Byte> imgEdit;
        private Image<Bgr, Byte> imgOverlay;
        private List<String> listResolution;
        private List<int> listFPS;
        private System.Threading.Timer timer;
        private System.Threading.TimerCallback timerDelegate;
        private List<double[]> laufPunkte;
        private List<double[]> messPunkte;

        /*Messen*/
        int maxMessPunkte = 2;
        
        /*Laufen*/
        int maxLaufPunkte = 1;

        public GUI()
        {
            InitializeComponent();
            _camera = new NAOCamera();
            _naoCamCurrentImageFormat = _camera.NaoCamImageFormats[1];
            timerDelegate = new TimerCallback(zeichneNaoBildInPictureBox);
        }

        private void btn_check_Click(object sender, EventArgs e)
        {
            #region Verbindung mit dem NAO überprüfen
            /*Überprüfen ob der NAO unter der angegebenen IP-Adresse verfügbar ist.
             *Es wird überprüft, ob eine Verbindung mit dem NAOqi des NAO hergestellt werden kann.
             *Nach einen Timeout von $timeout Sekunden wird der Verbindungsversuch abgebrochen und eine TimeoutException geworfen
             */
            int timeout = 2;
            using (TcpClient tcp = new TcpClient())
            {
                IAsyncResult ar = tcp.BeginConnect(tBnaoIP.Text, Convert.ToInt32(tBnaoPort.Text), null, null);
                System.Threading.WaitHandle wh = ar.AsyncWaitHandle;
                try
                {
                    if (!ar.AsyncWaitHandle.WaitOne(TimeSpan.FromSeconds(timeout), false))
                    {
                        tcp.Close();
                        throw new TimeoutException();
                    }
                    tcp.EndConnect(ar);
                    lblNaoStatus.Text = "NAO gefunden";
                    lblNaoStatus.ForeColor = Color.Green;
                }
                catch (TimeoutException ex)
                {
                    Console.Error.Write(ex.Message);
                    lblNaoStatus.Text = "NAO nicht gefunden";
                    lblNaoStatus.ForeColor = Color.Red;
                    return;
                }
                catch (SocketException ex)
                {
                    Console.Error.Write(ex.Message);
                    lblNaoStatus.Text = "NAO nicht gefunden";
                    lblNaoStatus.ForeColor = Color.Red;
                    return;
                }
                finally
                {
                    wh.Close();
                }
            }
            #endregion

            /* Kontroll- und Kameraobjekte erzeugen */
            _naoControl = new NAOControl(tBnaoIP.Text, Convert.ToInt32(tBnaoPort.Text));
            _camera = new NAOCamera();

            /* Batteriestand auslesen und anzeigen */
            checkBattery();

            /* Stiffness auslesen und anzeigen */
            if (checkStiffness())
            {
                lblStiffnessStatus.Text = "Enabled";
                lblStiffnessStatus.ForeColor = Color.Green;
            }
            else
            {
                lblStiffnessStatus.Text = "Disabled";
                lblStiffnessStatus.ForeColor = Color.Red;
            }

            /*Head-Pitch und Head-Yaw auslesen*/
            lblHeadPitchStatus.Text = "Head Pitch: " + Math.Round(_naoControl.rad2deg(_naoControl._motionProxy.getAngles("HeadPitch", true)[0]), 2);
            lblHeadYawStatus.Text = "Head Yaw: " + Math.Round(_naoControl.rad2deg(_naoControl._motionProxy.getAngles("HeadYaw", true)[0]),2);

            /* Mögliche Posen in die ComboBox schreiben */
            if (_naoControl.isRobotPostureAvailable())
            {
                cmbPoses.DataSource = _naoControl._robotPostureProxy.getPostureList();
            }            
            
            /* Auflösungen in die ComboBox schreiben */
            listResolution = new List<string>();
            for (int i = 0; i < _camera.NaoCamImageFormats.Count; i++)
            {
                listResolution.Add(_camera.NaoCamImageFormats.ElementAt(i).name);
            }
            cmbResolution.DataSource = listResolution;
            int resolutionWidth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[0]); ;
            int resolutionHeigth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[1]);
            _naoControl.setResolution(new int[] { resolutionWidth, resolutionHeigth });
            
            /* FPS in die ComboBox schreiben */
            listFPS = new List<int>();
            listFPS.Add(1); listFPS.Add(2);
            listFPS.Add(3); listFPS.Add(5);
            cmbFPS.DataSource = listFPS;
            _naoControl.setFPS(Convert.ToInt32(cmbFPS.Text));
            
            /* GroupBoxes freigeben */
            if (_naoControl.isMotionAvailable())
            {
                gBMotionControl.Enabled = true;
                gBCameraControl.Enabled = true;
                gBLaufen.Enabled = true;
            }
        }

        private void btn_execMotion_Click(object sender, EventArgs e)
        {
            if (_naoControl._robotPostureProxy != null)
            {
                List<string> liste = _naoControl._robotPostureProxy.getPostureList();
                if (liste != null)
                {
                    _naoControl._robotPostureProxy.goToPosture(Convert.ToString(cmbPoses.SelectedItem), 0.5f);
                    checkStiffness();
                }
            }
        }

        private void btn_connectCamera_Click(object sender, EventArgs e)
        {
            /*Erzeuge das ImageProcessor Objekt*/
            _imageProcessor = new ImageProcessor(_naoControl.getResolution());

            /*Bildformat der Kamera anhand der gewählten Auflösung auswählen*/
            for (int i = 0; i < _camera.NaoCamImageFormats.Count; i++)
            {
                if (_camera.NaoCamImageFormats.ElementAt(i).name == cmbResolution.Text)
                {
                    _naoCamCurrentImageFormat = _camera.NaoCamImageFormats.ElementAt(i);
                }
            }
            
            /*Mit der Kamera verbinden*/
            try
            {
                if (_camera.Connect(_naoControl.getIP(), _naoCamCurrentImageFormat, KCOLORSPACE, _naoControl.getFPS()))
                {
                    lblCameraStatus.Text = "Verbindung hergestellt";
                    lblCameraStatus.ForeColor = Color.Green;
                }
                timer = new System.Threading.Timer(timerDelegate);
                _naoCamInitialised = true;
                cbStandbild.Enabled = true;
            }
            catch (Exception ex)
            {
                _naoCamInitialised = false;
                lblCameraStatus.Text = "Fehler beim Verbinden";
                lblCameraStatus.ForeColor = Color.Red;
                Console.Out.WriteLine("Exception while connecting to NaoCam: " + ex);
            }
            /*Wenn die verbindung zu der Kamera erfolgreich war:
             * GUI Elemente Aktualisieren
             * Timer zum aktualisieren des Kamerabilds starten
             *Wenn die Checkbox Messen aktiviert ist:
             * Nur einmal das Bild aktualisieren
             */
            if (_naoCamInitialised)
            {
                timerGUI.Start();
                try
                {
                    if (_naoControl.DEBUG)
                    {
                        timer.Change(0, (int)Math.Ceiling(10000.0 / _naoControl.getFPS()));
                    }
                    if (cbStandbild.Checked)
                    {
                        timer.Change(0, System.Threading.Timeout.Infinite);
                    }
                    else
                    {
                        timer.Change(0, (int)Math.Ceiling(1000.0 / _naoControl.getFPS()));
                    }
                }
                catch (ObjectDisposedException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Disposed)" + ex.Message);
                }
                catch (ArgumentOutOfRangeException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Out of Range) " + ex.Message);
                }
            }
        }

        void UpdateScreen(object sender, EventArgs e)
        {
            _naoControl.berechneModell();
            lblHeadPitchStatus.Text = "HeadPitch: " + Math.Round(_naoControl.getCamAnglePitch(), 2);
            lblHeadYawStatus.Text = "HeadYaw: " + Math.Round(_naoControl.getCamAngleYaw(), 2);
            /*
            if (_naoControl.isBatteryAvailable() && !_naoControl.DEBUG)
            {
                lblBatteryPercent.Text = _naoControl._batteryProxy.getBatteryCharge() + " %";
            }
             * */
        }

        void zeichneNaoBildInPictureBox(object stateInfo)
        {
            if (_naoCamInitialised && !updatingPicture)
            {
                updatingPicture = true;
                /* Bild von der Kamera holen */
                bitmap = new Bitmap(_camera.GetImage(_naoCamCurrentImageFormat));
                imgOrig = new Image<Bgr, Byte>(bitmap);
                imgEdit = new Image<Bgr, Byte>(bitmap);
                imgOverlay = new Image<Bgr, Byte>(bitmap.Size);
                /* Ausgabebild bearbeiten */
                if (cbDistance.Checked)
                {
                    imgEdit = _imageProcessor.zeichneEntfernungen(imgEdit, _naoControl.berechneEntfernungen(), _naoControl.getResolution()[1]);
                }
                if (cbSURF.Checked)
                {
                    imgEdit = _imageProcessor.SURF(imgOrig, imgEdit,_naoControl.getCamHeigth());
                }
                if (cbHough.Checked)
                {
                    imgEdit = _imageProcessor.HoughLines(imgOrig, imgEdit);
                }
                /* Bilder zeichnen */
                imgBoxOrig.Image = imgOrig;
                imgBoxEdit.Image = imgEdit;
                /*Bildercache leeren*/
                bitmap = null;
            }
            updatingPicture = false;
        }

        private void toggleStiffness()
        {
            if (checkStiffness())
            {
                _naoControl._motionProxy.setStiffnesses("Body", 0.0f);
                lblStiffnessStatus.Text = "Disabled";
                lblStiffnessStatus.ForeColor = Color.Red;
            }
            else
            {
                _naoControl._motionProxy.setStiffnesses("Body", 1.0f);
                lblStiffnessStatus.Text = "Enabled";
                lblStiffnessStatus.ForeColor = Color.Green;
            }
        }

        private bool checkStiffness()
        {
            List<float> stiffnesses = _naoControl._motionProxy.getStiffnesses("Body");
            if (stiffnesses[0] == 1.0f)
            {
                lblStiffnessStatus.Text = "Enabled";
                lblStiffnessStatus.ForeColor = Color.Green;
                return true;
            }
            else
            {
                lblStiffnessStatus.Text = "Disabled";
                lblStiffnessStatus.ForeColor = Color.Red;
                return false;
            }
        }

        private void checkBattery()
        {
            if (_naoControl.isBatteryAvailable() && !_naoControl.DEBUG)
            {
                lblBatteryPercent.Text = "" + _naoControl._batteryProxy.getBatteryCharge() + " %";
            }
        }

        #region GUI-Events

        private void beendenToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (_camera != null)
            {
                _camera.Disconnect();
            }
            _naoControl.releaseProxies();
            System.Windows.Forms.Application.Exit();
        }

        private void btnDisconnectCamera_Click(object sender, EventArgs e)
        {
            /*Timer, der das Bild aktualisiert, löschen.*/
            try
            {
                if (_naoCamInitialised && !updatingPicture)
                {
                    timer.Change(System.Threading.Timeout.Infinite, System.Threading.Timeout.Infinite);
                    timer.Dispose();
                    timerGUI.Stop();
                    _camera.Disconnect();
                }
            }
            catch (ObjectDisposedException ex)
            {
                System.Console.WriteLine("Error while disposing timer: (Disposed)" + ex.Message);
            }
            catch (ArgumentOutOfRangeException ex)
            {
                System.Console.WriteLine("Error while disposing timer: (Out of Range) " + ex.Message);
            }
            
            if (!timerGUI.Enabled && _camera.isDisconnected())
            {
                imgBoxOrig.Image = new Image<Bgr, Byte>(NAO_Image_Processing.Properties.Resources.no_pic_1);
                imgBoxEdit.Image = new Image<Bgr, Byte>(NAO_Image_Processing.Properties.Resources.no_pic_2);
                lblCameraStatus.Text = "nicht Verbunden";
                lblCameraStatus.ForeColor = Color.Red;
            }
        }

        private void btn_ToggleStiffness_Click(object sender, EventArgs e)
        {
            toggleStiffness();
        }

        private void textBox1_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == '\r' && tBHeadPitch.Text != "")
            {
                try
                {
                    object name = "HeadPitch";
                    object change = (float)(_naoControl.deg2rad(Convert.ToDouble(tBHeadPitch.Text)));
                    float speed = 0.1f;
                    _naoControl._motionProxy.setStiffnesses("Head", 1.0f);
                    _naoControl._motionProxy.setAngles(name, change, speed);
                    lblHeadPitchStatus.Text = "Head Pitch: " + Math.Round(_naoControl.rad2deg(_naoControl._motionProxy.getAngles("HeadPitch", true)[0]), 2);
                }
                catch (FormatException ex)
                {
                    Console.Out.WriteLine("Exception while parsing HeadPitch: " + ex.Message);
                }
            }
        }

        private void tBHeadYaw_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == '\r' && tBHeadYaw.Text != "")
            {
                try
                {
                    object name = "HeadYaw";
                    object change = (float)(_naoControl.deg2rad(Convert.ToDouble(tBHeadYaw.Text)));
                    float speed = 0.1f;
                    _naoControl._motionProxy.setStiffnesses("Head", 1.0f);
                    _naoControl._motionProxy.setAngles(name, change, speed);
                    lblHeadYawStatus.Text = "Head Yaw: " + Math.Round(_naoControl.rad2deg(_naoControl._motionProxy.getAngles("HeadYaw", true)[0]), 2);
                }
                catch (FormatException ex)
                {
                    Console.Out.WriteLine("Exception while parsing HeadYaw: " + ex.Message);
                }
            }
        }

        private void imgBoxEdit_MouseClick(object sender, MouseEventArgs e)
        {
            if (_naoCamInitialised && !updatingPicture && cbStandbild.Checked && !cbLaufen.Checked && !cbMessen.Checked && e.Button == System.Windows.Forms.MouseButtons.Left)
            {
                updatingPicture = true;
                double[] mausKoordinaten = new double[2];
                double[] bodenKoordinaten = new double[2];
                double[] posAufBildflaeche = new double[2];
                double resolutionFactor;
                resolutionFactor = (double)_naoControl.getResolution()[0] / (double)imgBoxEdit.Width;
                mausKoordinaten[0] = Math.Ceiling(e.X * resolutionFactor);
                mausKoordinaten[1] = Math.Ceiling(e.Y * resolutionFactor);
                posAufBildflaeche = _imageProcessor.pixelNachPositionAufBildflaeche(mausKoordinaten);
                bodenKoordinaten = _imageProcessor.positionAufBildflaecheNachEntfernung(posAufBildflaeche, _naoControl.getCamHeigth());
                imgEdit = _imageProcessor.zeichnePunkt(imgEdit, imgOverlay, new Point((int)(mausKoordinaten[0]), (int)(mausKoordinaten[1])), NAOControl.euklidDistanz(bodenKoordinaten[0], bodenKoordinaten[1]));
            }

            /*Reaktion auf Linksklick*/
            if (_naoCamInitialised && !updatingPicture && (cbLaufen.Checked || cbMessen.Checked) && e.Button == System.Windows.Forms.MouseButtons.Left)
            {
                updatingPicture = true;
                double[] mausKoordinaten = new double[2];
                double[] bodenKoordinaten = new double[2];
                double[] posAufBildflaeche = new double[2];
                double resolutionFactor = (double)_naoControl.getResolution()[0] / (double)imgBoxEdit.Width;
                mausKoordinaten[0] = Math.Ceiling(e.X * resolutionFactor);
                mausKoordinaten[1] = Math.Ceiling(e.Y * resolutionFactor);
                posAufBildflaeche = _imageProcessor.pixelNachPositionAufBildflaeche(mausKoordinaten);
                bodenKoordinaten = _imageProcessor.positionAufBildflaecheNachEntfernung(posAufBildflaeche, _naoControl.getCamHeigth());

                #region Laufen
                if (cbLaufen.Checked)
                {
                    if (laufPunkte.Count == maxLaufPunkte)
                    {
                        laufPunkte = new List<double[]>();
                    }
                    if (laufPunkte.Count < maxLaufPunkte)
                    {
                        laufPunkte.Add(bodenKoordinaten);
                        imgEdit = _imageProcessor.zeichnePunkt(imgEdit, imgOverlay, new Point((int)(mausKoordinaten[0]), (int)(mausKoordinaten[1])), NAOControl.euklidDistanz(laufPunkte[0][0], laufPunkte[0][1]));
                    }
                }
                #endregion

                #region Messen
                if (cbMessen.Checked)
                {
                    /*Wenn Bereits zwei Punkte ausgewählt wurden, soll kein Punkt mehr aufgenommen werden.
                     *Zunächst müssen über ein Rechtsklick die Punkte entfernt werden.
                     */
                    if (messPunkte.Count == maxMessPunkte)
                    {
                        return;
                    }
                    /*Wenn die maximale Anzahl an Punkten noch nicht erreicht ist,
                     * soll der Punkt zu den MessPunkten hinzugefügt werden.
                     */
                    if (messPunkte.Count < maxMessPunkte)
                    {
                        messPunkte.Add(bodenKoordinaten);
                        /*Ist die maximale Anzahl mit dem aktuellen Punkt erreicht,
                         * soll die Entfernung zwischen beiden Punkten am zweiten Punkt eingezeichnet werden.
                         * Sonst soll einfach der Punkt eingezeichnet werden.
                         */
                        if (messPunkte.Count == maxMessPunkte)
                        {
                            imgEdit = _imageProcessor.zeichnePunkt(imgEdit, imgOverlay, new Point((int)(mausKoordinaten[0]), (int)(mausKoordinaten[1])), _naoControl.euklidDistanz(messPunkte));
                        }
                        else
                        {
                            imgEdit = _imageProcessor.zeichnePunkt(imgEdit, imgOverlay, new Point((int)(mausKoordinaten[0]), (int)(mausKoordinaten[1])));
                        }
                    }
                }
                #endregion
            }

            /*Reaktion auf Mittelklick*/
            if (_naoCamInitialised && !updatingPicture && e.Button == System.Windows.Forms.MouseButtons.Middle)
            {
                messPunkte = new List<double[]>();
                laufPunkte = new List<double[]>();
                imgEdit = imgOrig;
                imgOverlay = new Image<Bgr, Byte>(imgOverlay.Size);
                zeichneNaoBildInPictureBox(sender);
                return;
            }
            imgBoxEdit.Image = imgEdit;
            updatingPicture = false;
        }

        private void cbStandbild_CheckedChanged(object sender, EventArgs e)
        {
            if (cbStandbild.Checked)
            {
                try
                {
                    timer.Change(System.Threading.Timeout.Infinite, System.Threading.Timeout.Infinite);
                    btnBildAktualisieren.Enabled = true;
                    cmbFPS.Enabled = false;
                    cbMessen.Enabled = true;
                    cbLaufen.Enabled = true;
                    /*Punkte zurücksetzen*/
                    laufPunkte = new List<double[]>();
                    messPunkte = new List<double[]>();
                }
                catch (ObjectDisposedException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Disposed)" + ex.Message);
                }
                catch (ArgumentOutOfRangeException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Out of Range) " + ex.Message);
                }
            }
            else
            {
                try
                {
                    timer.Change(0, (int)Math.Ceiling(1000.0 / _naoControl.getFPS()));
                    btnBildAktualisieren.Enabled = false;
                    cmbFPS.Enabled = true;
                    cbMessen.Enabled = false;
                    cbLaufen.Enabled = false;
                }
                catch (ObjectDisposedException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Disposed)" + ex.Message);
                }
                catch (ArgumentOutOfRangeException ex)
                {
                    System.Console.WriteLine("Error while disposing timer: (Out of Range) " + ex.Message);
                }
            }
        }

        private void überToolStripMenuItem_Click(object sender, EventArgs e)
        {
            MessageBox.Show("NAO-Control\n" +
                            "An der Fachhochschule Dortmund\n" +
                            "im Fachbereich Informatik\n" +
                            "Studiengang Technische Informatik\n" +
                            "erstellte Thesis\n" +
                            "zur Erlangung des akademischen Grades\n" +
                            "Master of Science (M.Sc.)\n\n" +
                            "Betreuung durch: Prof. Dr. Gisela Schäfer-Richter\n" +
                            "©2013", "Über", MessageBoxButtons.OK);
        }

        private void cmbResolution_SelectedValueChanged(object sender, EventArgs e)
        {
            if (cmbResolution.Text != null)
            {
                int resolutionWidth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[0]);;
                int resolutionHeigth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[1]);
                _naoControl.setResolution(new int[] {resolutionWidth,resolutionHeigth});
            }
        }

        private void cmbFPS_SelectedValueChanged(object sender, EventArgs e)
        {
            if (cmbFPS.Text != null)
            {
                _naoControl.setFPS(Convert.ToInt32(cmbFPS.Text));
            }
        }

        private void btnBildAktualisieren_Click(object sender, EventArgs e)
        {
            zeichneNaoBildInPictureBox(sender);
        }

        private void cbMessen_CheckedChanged(object sender, EventArgs e)
        {
            /*Laufen deaktivieren, wenn Messen aktiv ist.*/
            if (cbMessen.Checked)
            {
                cbLaufen.Enabled = false;
            }
            else
            {
                cbLaufen.Enabled = true;
            }
        }

        private void cbLaufen_CheckedChanged(object sender, EventArgs e)
        {
            /*Messen deaktivieren, wenn Laufen aktiv ist.*/
            if (cbLaufen.Checked)
            {
                cbMessen.Enabled = false;
            }
            else
            {
                cbMessen.Enabled = true;
            }
        }
        #endregion

        private void btnLaufen_Click(object sender, EventArgs e)
        {
            _naoControl._motionProxy.moveInit();
            _naoControl._motionProxy.post.moveTo((float)laufPunkte[0][1] / 100.0f, -(float)laufPunkte[0][0] / 100.0f, 0.0f);
            _naoControl._robotPostureProxy.goToPosture("StandInit", 0.5f);
        }

        private void cbDistance_CheckedChanged(object sender, EventArgs e)
        {
            if (cbDistance.Checked)
            {
                cbSURF.Enabled = true;
                cbHough.Enabled = true;
                cbStandbild.Enabled = true;
                tBHeadPitch.Enabled = true;
                tBHeadYaw.Enabled = true;
            }
        }

        private void btnSchuss_Click(object sender, EventArgs e)
        {
            double[] ballKanteAufBodenInPixel = new double[2];
            double[] bodenKoordinaten = new double[2];
            double[] posAufBildflaeche = new double[2];
            zeichneNaoBildInPictureBox(sender);
            IntPtr imgThreshold = _imageProcessor.holeSchwellwertBild(imgOrig);
            Image<Bgr, Byte> imgHoughCircles = _imageProcessor.zeichneHoughKreise(imgThreshold, imgEdit);
            CircleF ballKreis = _imageProcessor.getKreisBall();
            ballKanteAufBodenInPixel[0] = Math.Abs(ballKreis.Center.X);
            ballKanteAufBodenInPixel[1] = ballKreis.Center.Y + ballKreis.Radius;
            posAufBildflaeche = _imageProcessor.pixelNachPositionAufBildflaeche(ballKanteAufBodenInPixel);
            bodenKoordinaten = _imageProcessor.positionAufBildflaecheNachEntfernung(posAufBildflaeche, _naoControl.getCamHeigth());
            _imageProcessor.zeichnePunkt(imgHoughCircles, null, new Point((int)ballKanteAufBodenInPixel[0], (int)ballKanteAufBodenInPixel[1]), NAOControl.euklidDistanz(bodenKoordinaten[0], bodenKoordinaten[1]));
            imgBoxEdit.Image = imgHoughCircles;
            
            /*NAO zu position laufen lassen*/
            /*    X [1]
             *    |
             *    |
             * ___|___Y [0]
             * 
             * 
             */
            _naoControl._motionProxy.moveInit();
            _naoControl._motionProxy.post.moveTo((float)(bodenKoordinaten[1]/ 100.0f)*0.8f, -(float)(bodenKoordinaten[0] / 100.0f)*1.45f, 0.0f);
            _naoControl._robotPostureProxy.goToPosture("StandInit", 0.5f);
            List<String> verhalten = _naoControl._behaviourManagerProxy.getBehaviorNames();
            foreach (String name in verhalten)
            {
                if (name == "User/Kick")
                {
                    _naoControl._behaviourManagerProxy.runBehavior(name);
                }
            }
            _naoControl._robotPostureProxy.goToPosture("Stand", 0.5f);
        }

        private void btnPunkteLoeschen_Click(object sender, EventArgs e)
        {
            messPunkte = new List<double[]>();
            laufPunkte = new List<double[]>();
            imgEdit = imgOrig;
            imgOverlay = new Image<Bgr, Byte>(imgOverlay.Size);
            zeichneNaoBildInPictureBox(sender);
        }
    }
}